#include "StdAfx.h"
#include "MillingTool.h"
#include <math.h>
#include <algorithm>
using namespace std;

#define POINT_EQUALS(p1,p2,equal){ equal = (p1.x==p2.x && p1.y==p2.y && p1.z==p2.z);}

MillingTool::MillingTool(double length,int diameter)
{
	this->length = length;
	this->diameter = diameter;

	this->referencePoint.x = 0.0;
	this->referencePoint.y = 0.0;
	this->referencePoint.z = 0.0;
}

MillingTool::~MillingTool(void)
{
}

void MillingTool::MoveTo(double x,double y,double z)
{
	this->velocity.x = (MY_DATATYPE) (x-this->posX);
	this->velocity.y = (MY_DATATYPE) (y-this->posY);
	this->velocity.z = (MY_DATATYPE) (z-this->posZ);

	Vector4D zeroVector;
	if(abs(this->velocity.x-zeroVector.x) > EPSILON || abs(this->velocity.y - zeroVector.y) > EPSILON || abs(this->velocity.z - zeroVector.z) > EPSILON)
		this->velocity =  Vector4D::normalize(this->velocity);

	this->posX = x;
	this->posY = y;
	this->posZ = z;
}

void MillingTool::performFloodFill(Point3D & p,hash_set<int> *& visitedPoints,vector<Point3D>*& result)
{
	std::hash_set<int>::iterator it;
	if(surpassMovementDirectionCond(p,visitedPoints))
		result->push_back(Point3D(p.x,p.y,p.z));
	visitedPoints->insert(p.getHashCode2D());
	Point3D ps(p.x-1,p.y,p.z);

	if(surpassMovementDirectionCond(ps,visitedPoints))
		performFloodFill(ps,visitedPoints,result);
	ps.x = p.x+1;

	if(surpassMovementDirectionCond(ps,visitedPoints))
		performFloodFill(ps,visitedPoints,result);

	ps.x = p.x;
	ps.y = p.y - 1;

	if(surpassMovementDirectionCond(ps,visitedPoints))
		performFloodFill(ps,visitedPoints,result);

	ps.y = p.y + 1;
	if(surpassMovementDirectionCond(ps,visitedPoints))
		performFloodFill(ps,visitedPoints,result);
}

vector<Point3D>* MillingTool::GetToolSurfacePoints()
{
	hash_set<int> * set = new hash_set<int>();
	vector<Point3D> * result = new vector<Point3D>();
	Point3D middle((MY_DATATYPE)this->posX,(MY_DATATYPE)this->posY,(MY_DATATYPE)this->posZ);
	performFloodFill(middle,set,result);
	set->clear();
	delete set;
	return result;
}

void MillingTool::adjustDirection(const Point3D & from,const Point3D & to)
{
	this->direction.x = to.x - from.x;
	this->direction.y = to.y - from.y;
	this->direction.z = to.z - from.z;
	if(abs(this->direction.x - 0.0)>EPSILON && abs(this->direction.y - 0.0)>EPSILON && abs(this->direction.z - 0.0)>EPSILON)
		this->direction = Vector4D::normalize(this->direction);
}